#ifndef SERVO_UTILS_H
#define SERVO_UTILS_H

#include <avr/io.h>

#define CHANNEL_NB 8

#define SERVO_DDR DDRC
#define SERVO_PORT PORTC
#define SERVO_PIN PC5

#define SERVO_MIN 1500
#define SERVO_MAX 4500

#define AILERON_CH 0
#define PROFUNDOR_CH 1
#define MOTOR_CH 2
#define RUDDER_CH 3
#define MODE_CH 4

#define SET_SERVO(ddr,pin) ((ddr) |= (1<<pin))
#define PWM_HIGH(port,pin) ((port) |= (1<<pin))
#define PWM_LOW(port,pin) ((port) &= ~(1<<pin))

#define MOVE_SERVO_TO(des_pos)				\
  ({des_pos = saturate(des_pos,SERVO_MIN,SERVO_MAX);	\
    SERVO_POS_CNTS = pos_to_cnts(des_pos);})

extern volatile unsigned int ppm_vals[CHANNEL_NB]; //servo positions
extern volatile uint16_t H_COUNTER2; // High Register for Counter TCNT2
extern volatile int SERVO_POS_CNTS; // Servo position in counts (62~125)

extern void servo_init(void);
extern int saturate(int pos, int min, int max);
extern int pos_to_cnts(int pos);

#endif /* SERVO_UTILS_H */
